from utils.general_pub_sub_node import *


class MoveArmPublisher(Publisher):

    def __init__(self, name, msg_typ, topic, timer_msg=None, timer_period=0):
        super().__init__(name,
                         msg_typ,
                         topic,
                         timer_msg=timer_msg,
                         timer_period=timer_period)

    def set_goal(self, x, y, z):
        set_goal_msg = PoseStamped()
        set_goal_msg.header.frame_id = 'link6'
        set_goal_msg.pose.position.x = x
        set_goal_msg.pose.position.y = y
        set_goal_msg.pose.position.z = z
        set_goal_msg.pose.orientation.w = 1.0

        self.normal_publish(set_goal_msg)
